/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_NLOPTMOTIONCONVERTER_H_
#define __MMM_NLOPTMOTIONCONVERTER_H_

#include <nlopt.hpp>

#include <MMM/MMMCore.h>
#include <VirtualRobot/VirtualRobotCommon.h>
#include "ConvertingStrategy.h"
#include "../../MotionConverter.h"

namespace MMM
{

/*!
    \brief A converter using NLopt to convert Vicon marker motions to the MMM format.
*/
class MMM_IMPORT_EXPORT NloptMotionConverter : public MotionConverter
{
public:
    enum OptimizationStrategy {
        FRAMEWISE_LOCAL,  /* Optimize each frame by itself, do not consider inter-frame constraints/objective */
        WHOLE_MOTION /* Optimize the whole motion over segments of a motion at once,*/
    };

    NloptMotionConverter(MotionPtr inputMotion, ModelPtr outputModel, ModelPtr outputModelUnprocessed,
                         ModelProcessorPtr outputModelProcessor, const std::string &configFile,
                         const std::string &convertedMotionName = std::string(),
                         OptimizationStrategy optimizationStrategy = FRAMEWISE_LOCAL);

    void cancel();

    float getCurrentTimestep();

    MotionPtr convertMotion();

    static constexpr const char* NAME = "NloptConverter";

private:

    void readConverterConfig();
    std::map<float, std::map<std::string, Eigen::Vector3f> > getLabeledMarkerData();

    OptimizationStrategy optimizationStrategy;
    ConvertingStrategyPtr strategy;

    std::map<std::string, std::string> markerMapping;
    std::map<std::string, float> markerWeights;
    float markerScaleFactor;
    float targetModelSize;
};

typedef boost::shared_ptr<NloptMotionConverter> NloptMotionConverterPtr;

}

#endif 
